/*--------------------------------------------- Noble Steed Engine--------------------------------------*
Authors : Daniel Randle, Alex Bourne
Date Created : Mar 5 2013

File:
	nsmath.cpp

Description:
	This file contains the definition for the math fucnctions vector/matrice transforms
*-----------------------------------------------------------------------------------------------------*/

#include <nsmath3d.h>
#include <math.h>
#include <logfile.h>
#include <sstream>


NSVec2Df::NSVec2Df():u(0.0f), v(0.0f), x(u), y(v)
{}

NSVec2Df::NSVec2Df(const NSVec2Df & copy):u(copy.u), v(copy.v), x(u), y(v)
{}

NSVec2Df::NSVec2Df(float _u, float _v): u(_u), v(_v), x(u), y(v)
{}

std::string NSVec2Df::toString()
{
	std::stringstream ss;
	ss << "x : " << x << "   y : " << y;
	return ss.str();
}

NSVec2Df NSVec2Df::operator+(const NSVec2Df & rhsVector) const
{
	return NSVec2Df(u + rhsVector.u, v + rhsVector.v);
}

NSVec2Df NSVec2Df::operator-(const NSVec2Df & rhsVector) const
{
	return NSVec2Df(u - rhsVector.u, v - rhsVector.v);
}

NSVec2Df & NSVec2Df::operator=(const NSVec2Df & rhsVector)
{
	if (this == &rhsVector)
		return *this;
	u = rhsVector.u;
	v = rhsVector.v;
	return *this;
}

NSVec2Df & NSVec2Df::operator+=(const NSVec2Df & rhsVector)
{
	u += rhsVector.u;
	v += rhsVector.v;
	return *this;
}

NSVec2Df & NSVec2Df::operator-=(const NSVec2Df & rhsVector)
{
	u -= rhsVector.u;
	v -= rhsVector.v;
	return *this;
}

bool NSVec2Df::operator==(const NSVec2Df & rhsVector) const
{
	return ( (u == rhsVector.u) && (v == rhsVector.v) );
}

bool NSVec2Df::operator!=(const NSVec2Df & rhsVector) const
{
	return !(*this == rhsVector);
}


NSVec2Df NSVec2Df::operator*(const float rhsScalar) const
{
	return NSVec2Df(u * rhsScalar, v * rhsScalar);
}

NSVec2Df NSVec2Df::operator/(const float rhsScalar) const
{
	return NSVec2Df(u / rhsScalar, v / rhsScalar);
}

NSVec2Df & NSVec2Df::operator*=(const float rhsScalar)
{
	return *this = (*this * rhsScalar);
}

NSVec2Df & NSVec2Df::operator/=(const float rhsScalar)
{
	return *this = (*this / rhsScalar);
}


NSVec3Df::NSVec3Df():x(0.0f),y(0.0f),z(0.0f)
{}

NSVec3Df::NSVec3Df(const NSVec3Df & xyz): x(xyz.x), y(xyz.y), z(xyz.z)
{}

NSVec3Df::NSVec3Df(const NSVec2Df & xy, float _z): x(xy.x), y(xy.y), z(_z)
{}

NSVec3Df::NSVec3Df(float _x, const NSVec2Df & yz): x(_x), y(yz.x), z(yz.y)
{}

NSVec3Df::NSVec3Df(float _x, float _y, float _z): x(_x), y(_y), z(_z)
{}

// Functions

void NSVec3Df::normalize()
{
	(*this) /= length();
}

float NSVec3Df::length()
{
	return  sqrtf(x*x + y*y + z*z);
}

void NSVec3Df::rotate(const NSVec3Df & amount)
{
	NSVec4Df rotationVec(*this,1.0f);
	rotationVec.rotate(amount);
	x = rotationVec.x; y = rotationVec.y; z = rotationVec.z;
}

void NSVec3Df::rotate(float xAxisAng, float yAxisAng, float zAxisAng)
{
	rotate(NSVec3Df(xAxisAng,yAxisAng,zAxisAng));
}

void NSVec3Df::add(float _x, float _y, float _z)
{
	(*this) += NSVec3Df(_x,_y,_z);
}

void NSVec3Df::set(float _x, float _y, float _z)
{
	x = _x; y = _y; z = _z;
}

float NSVec3Df::angleFrom(const NSVec3Df & otherVec) const
{
	return 0;
}

std::string NSVec3Df::toString()
{
	std::stringstream ss;
	ss << "x : " << x << "   y : " << y << "   z : " << z;
	return ss.str();
}

NSVec3Df NSVec3Df::cross(const NSVec3Df & crossedWith) const
{
    const float _x = y * crossedWith.z - z * crossedWith.y;
    const float _y = z * crossedWith.x - x * crossedWith.z;
    const float _z = x * crossedWith.y - y * crossedWith.x;

    return NSVec3Df(_x, _y, _z);
}


// Overloaded Operators

NSVec3Df NSVec3Df::operator+(const NSVec3Df & rhsVector) const
{
	return NSVec3Df(x+rhsVector.x, y+rhsVector.y, z+rhsVector.z);
}

NSVec3Df  NSVec3Df::operator-(const NSVec3Df & rhsVector) const
{
	return NSVec3Df(x-rhsVector.x, y-rhsVector.y, z-rhsVector.z);
}

NSVec3Df &  NSVec3Df::operator=(const NSVec3Df & rhsVector)
{
	if (this == &rhsVector)
		return *this;

	x = rhsVector.x;
	y = rhsVector.y;
	z = rhsVector.z;
	return *this;
}

NSVec3Df &  NSVec3Df::operator+=(const NSVec3Df & rhsVector)
{
	return ( (*this) = ( (*this) + rhsVector ) );
}

NSVec3Df &  NSVec3Df::operator-=(const NSVec3Df & rhsVector)
{
	return ( (*this) = ( (*this) - rhsVector ) );
}

bool  NSVec3Df::operator==(const NSVec3Df & rhsVector) const
{
	return ( x == rhsVector.x && y == rhsVector.y && x == rhsVector.z);
}

bool  NSVec3Df::operator!=(const NSVec3Df & rhsVector) const
{
	return !(*this == rhsVector);
}

float  NSVec3Df::operator*(const NSVec3Df & rhsVector) // return dot product
{
	return ((x*rhsVector.x) + (y*rhsVector.y) + (z*rhsVector.z));
}

NSVec3Df  NSVec3Df::operator*(const float rhsScalar) const
{
	return NSVec3Df(x*rhsScalar, y*rhsScalar, z*rhsScalar);
}

NSVec3Df  NSVec3Df::operator/(const float rhsScalar) const
{
	return (*this) * ( 1 / rhsScalar);
}

NSVec3Df &  NSVec3Df::operator*=(const float rhsScalar)
{
	return ( (*this) = ( (*this) * rhsScalar ) );
}

NSVec3Df &  NSVec3Df::operator/=(const float rhsScalar)
{
	return ( (*this) = ( (*this) / rhsScalar ) );
}

// NSVec3DF Ends

// NSVec4DF Begins

 // Constructors
NSVec4Df::NSVec4Df():x(0.0f),y(0.0f),z(0.0f),w(0.0)
{}

NSVec4Df::NSVec4Df(const NSVec4Df & xyzw): x(xyzw.x), y(xyzw.y), z(xyzw.z), w(xyzw.w)
{}
NSVec4Df::NSVec4Df(const NSVec3Df & xyz, float _w): x(xyz.x), y(xyz.y), z(xyz.z), w(_w)
{}
NSVec4Df::NSVec4Df(float _x, const NSVec3Df & yzw): x(_x), y(yzw.x), z(yzw.y), w(yzw.z)
{}
NSVec4Df::NSVec4Df(const NSVec2Df & xy, const NSVec2Df & zw): x(xy.x), y(xy.y), z(zw.x), w(zw.y)
{}
NSVec4Df::NSVec4Df(const NSVec2Df & xy, float _z, float _w): x(xy.x), y(xy.y), z(_z), w(_w)
{}
NSVec4Df::NSVec4Df(float _x, const NSVec2Df & yz, float _w): x(_x), y(yz.x), z(yz.y), w(_w)
{}
NSVec4Df::NSVec4Df(float _x, float _y, const NSVec2Df & zw): x(_x), y(_y), z(zw.x), w(zw.y)
{}
NSVec4Df::NSVec4Df(float _x, float _y, float _z, float _w): x(_x), y(_y), z(_z), w(_w)
{}

// Functions

void NSVec4Df::normalize()
{
	(*this) /= length();
}

float NSVec4Df::length()
{
	return  sqrtf(x*x + y*y + z*z + w*w);
}

void NSVec4Df::rotate(const NSVec3Df & amount)
{
	*this = NSMatrix4Df::getRotationMatrix(amount) * (*this);
}

void NSVec4Df::rotate(float xAxisAng, float yAxisAng, float zAxisAng)
{
	rotate(NSVec3Df(xAxisAng,yAxisAng,zAxisAng));
}

void NSVec4Df::add(float _x, float _y, float _z, float _w)
{
	(*this) += NSVec4Df(_x,_y,_z,_w);
}

void NSVec4Df::set(float _x, float _y, float _z, float _w)
{
	x = _x; y = _y; z = _z; w = _w;
}

float NSVec4Df::angleFrom(const NSVec4Df & otherVec) const
{
	return 0;
}

std::string NSVec4Df::toString()
{
	std::stringstream ss;
	ss << "x : " << x << "   y : " << y << "   z : " << z << "   w : " << w;
	return ss.str();
}


// Overloaded Operators

NSVec4Df NSVec4Df::operator+(const NSVec4Df & rhsVector) const
{
	return NSVec4Df(x+rhsVector.x, y+rhsVector.y, z+rhsVector.z, w+rhsVector.w);
}

NSVec4Df  NSVec4Df::operator-(const NSVec4Df & rhsVector) const
{
	return NSVec4Df(x-rhsVector.x, y-rhsVector.y, z-rhsVector.z, w-rhsVector.w);
}

NSVec4Df &  NSVec4Df::operator=(const NSVec4Df & rhsVector)
{
	if (this == &rhsVector)
		return *this;

	x = rhsVector.x;
	y = rhsVector.y;
	z = rhsVector.z;
	w = rhsVector.w;
	return *this;
}

NSVec4Df &  NSVec4Df::operator+=(const NSVec4Df & rhsVector)
{
	return ( (*this) = ( (*this) + rhsVector ) );
}

NSVec4Df &  NSVec4Df::operator-=(const NSVec4Df & rhsVector)
{
	return ( (*this) = ( (*this) - rhsVector ) );
}
 
bool  NSVec4Df::operator==(const NSVec4Df & rhsVector) const
{
	return ( x == rhsVector.x && y == rhsVector.y && x == rhsVector.z && w == rhsVector.w );
}

bool  NSVec4Df::operator!=(const NSVec4Df & rhsVector) const
{
	return !(*this == rhsVector);
}

float NSVec4Df::operator*(const NSVec4Df & rhsVector)  const// return dot product
{
	return ((x*rhsVector.x) + (y*rhsVector.y) + (z*rhsVector.z) + (w*rhsVector.w));
}

NSVec4Df  NSVec4Df::operator*(const float & rhsScalar) const
{
	return NSVec4Df(x*rhsScalar, y*rhsScalar, z*rhsScalar, w*rhsScalar);
}

NSVec4Df  NSVec4Df::operator/(float rhsScalar) const
{
	return (*this) * ( 1 / rhsScalar);
}

NSVec4Df &  NSVec4Df::operator*=(float rhsScalar)
{
	return ( (*this) = ( (*this) * rhsScalar ) );
}

NSVec4Df &  NSVec4Df::operator/=(float rhsScalar)
{
	*this = ( *this / rhsScalar );
	return *this;
}



// Matrix 3D (3x3)

NSMatrix3Df::NSMatrix3Df():rows(MATDIM3D),
	columns(MATDIM3D)
{
	data[0][0] = 1.0f; data[0][1] = 0.0f; data[0][2] = 0.0f;
	data[1][0] = 0.0f; data[1][1] = 1.0f; data[1][2] = 0.0f;
	data[2][0] = 0.0f; data[2][1] = 0.0f; data[2][2] = 1.0f;
}

NSMatrix3Df::NSMatrix3Df(const NSMatrix3Df & copy):rows(MATDIM3D),
	columns(MATDIM3D)
{
	(*this) = copy;
}

NSMatrix3Df::NSMatrix3Df(NSVec3Df row1, NSVec3Df row2, NSVec3Df row3):rows(MATDIM3D),
	columns(MATDIM3D)
{
	setData(row1,row2,row3);
}

void NSMatrix3Df::normalizeAll()
{
	NSVec3Df row1(getRow(0));
	NSVec3Df row2(getRow(1));
	NSVec3Df row3(getRow(2));
	row1.normalize();
	row2.normalize();
	row3.normalize();
	setData(row1,row2,row3);
}

void NSMatrix3Df::setData(const NSVec3Df & row1,const NSVec3Df & row2,const NSVec3Df & row3)
{
	data[0][0] = row1.x; data[0][1] = row1.y; data[0][2] = row1.z;
	data[1][0] = row2.x; data[1][1] = row2.y; data[1][2] = row2.z;
	data[2][0] = row3.x; data[2][1] = row3.y; data[2][2] = row3.z;
}

void NSMatrix3Df::setRow(const NSVec3Df & vec, int row)
{
	if (row < 0 || row >= rows)
	{
		LogFile("Math3D error : NSMatrix3Df::setRow row index out of range");
		return;
	}

	data[row][0] = vec.x;
	data[row][1] = vec.y;
	data[row][2] = vec.z;
}

void NSMatrix3Df::setColumn(const NSVec3Df & vec, int column)
{
	if (column < 0 || column >= columns)
	{
		LogFile("Math3D error : NSMatrix3Df::setColumn column index out of range");
		return;
	}

	data[0][column] = vec.x;
	data[1][column] = vec.y;
	data[2][column] = vec.z;
}

float & NSMatrix3Df::value(int row, int column)
{
	return data[row][column];
}

const float & NSMatrix3Df::value(int row, int column) const
{
	return data[row][column];
}

NSVec3Df NSMatrix3Df::getRow(int row) const
{
	if (row < 0 || row >= rows)
	{
		LogFile("Math3D error : NSMatrix3Df::getRow row index out of range");
		return NSVec3Df();
	}

	return NSVec3Df(data[row][0], data[row][1], data[row][2]);
}

NSVec3Df NSMatrix3Df::getColumn(int column) const
{
	if (column < 0 || column >= columns)
	{
		LogFile("Math3D error : NSMatrix3Df::getColumn column index out of range");
		return NSVec3Df();
	}

	return NSVec3Df(data[0][column], data[1][column], data[2][column]);
}

const float * NSMatrix3Df::getDataPtr()
{
	return &data[0][0];
}

	// Overloaded operators
NSMatrix3Df NSMatrix3Df::operator*(const NSMatrix3Df & rhsMatrix) const
{
	NSMatrix3Df matrix;
	for (int count = 0; count < rows; ++count)
	{
		for (int i = 0; i < rows; ++i)
		{
			float val = 0.0f;
			for (int j = 0; j < columns; ++j)
				 val += data[count][j] * rhsMatrix.data[j][i];

			matrix.data[count][i] = val;
		}
	}
	return matrix;
}

NSMatrix3Df NSMatrix3Df::operator+(const NSMatrix3Df & rhsMatrix) const
{
	NSMatrix3Df matrix;
	for (int i = 0; i < rows; ++i)
	{
		for (int j = 0; j < columns; ++j)
			matrix.data[i][j] = data[i][j] + rhsMatrix.data[i][j];
	}
	return matrix;
}

NSMatrix3Df NSMatrix3Df::operator-(const NSMatrix3Df & rhsMatrix) const
{
	NSMatrix3Df matrix;
	for (int i = 0; i < rows; ++i)
	{
		for (int j = 0; j < columns; ++j)
			matrix.data[i][j] = data[i][j] - rhsMatrix.data[i][j];
	}
	return matrix;
}

// less than and greater than not supported
bool NSMatrix3Df::operator==(const NSMatrix3Df & rhsMatrix) const
{
	for (int i = 0; i < rows; ++i)
	{
		for (int j = 0; j < columns; ++j)
		{
			if ( data[i][j] != rhsMatrix.data[i][j] )
				return false;
		}
	}
	return true;
}

bool NSMatrix3Df::operator!=(const NSMatrix3Df & rhsMatrix) const
{
	return !( (*this) == rhsMatrix );
}

NSMatrix3Df & NSMatrix3Df::operator=(const NSMatrix3Df & rhsMatrix)
{
	// copy every data element
	if (this == &rhsMatrix)
		return *this;

	for (int i = 0; i < rows; ++i)
	{
		for (int j = 0; j < columns; ++j)
			data[i][j] = rhsMatrix.data[i][j];
	}
	return *this;
}

NSMatrix3Df & NSMatrix3Df::operator+=(const NSMatrix3Df & rhsMatrix)
{
	return ( (*this) = ( (*this) + rhsMatrix ) );
}

NSMatrix3Df & NSMatrix3Df::operator-=(const NSMatrix3Df & rhsMatrix)
{
	return ( (*this) = ( (*this) - rhsMatrix ) );
}

NSMatrix3Df & NSMatrix3Df::operator*=(const NSMatrix3Df & rhsMatrix)
{
	return ( (*this) = ( (*this) * rhsMatrix ) );
}

// Overloaded for 3D vector
NSVec3Df NSMatrix3Df::operator*(const NSVec3Df & rhsVec) const
{
	NSVec3Df vec;
	float orig[MATDIM3D];
	float arr[MATDIM3D];
	orig[0] = rhsVec.x; orig[1] = rhsVec.y; orig[2] = rhsVec.z;
	for (int i = 0; i < rows; ++i)
	{
		float val = 0.0f;
		for (int j = 0; j < columns; ++j )
			val += data[i][j] * orig[j];

		arr[i] = val;
	}
	return NSVec3Df(arr[0], arr[1], arr[2]);
}


NSMatrix3Df NSMatrix3Df::operator*(float rhsScalar) const
{
	NSMatrix3Df ret;
	for (int i = 0; i < rows; ++i)
	{
		for (int j = 0; j < columns; ++j)
			ret.value(i,j) = value(i,j) * rhsScalar;
	}
	return ret;
}

NSMatrix3Df NSMatrix3Df::operator/(float rhsScalar) const
{
	return *this * (1 / rhsScalar);
}

 // NSMatrix4Df

NSMatrix4Df::NSMatrix4Df():rows(MATDIM4D), 
	columns(MATDIM4D)
{
	data[0][0] = 1.0f; data[0][1] = 0.0f; data[0][2] = 0.0f; data[0][3] = 0.0f;
	data[1][0] = 0.0f; data[1][1] = 1.0f; data[1][2] = 0.0f; data[1][3] = 0.0f;
	data[2][0] = 0.0f; data[2][1] = 0.0f; data[2][2] = 1.0f; data[2][3] = 0.0f;
	data[3][0] = 0.0f; data[3][1] = 0.0f; data[3][2] = 0.0f; data[3][3] = 1.0f;
}

NSMatrix4Df::NSMatrix4Df(const NSMatrix4Df & copy): rows(MATDIM4D), 
	columns(MATDIM4D)
{
	(*this) = copy;
}

NSMatrix4Df::NSMatrix4Df(const NSVec4Df & vec1, const NSVec4Df & vec2, const NSVec4Df & vec3, const NSVec4Df & vec4): rows(MATDIM4D), 
	columns(MATDIM4D)
{
	setData(vec1,vec2,vec3,vec4);
}

void NSMatrix4Df::normalizeAll()
{
	NSVec4Df row1(getRow(0));
	NSVec4Df row2(getRow(1));
	NSVec4Df row3(getRow(2));
	NSVec4Df row4(getRow(3));
	row1.normalize();
	row2.normalize();
	row3.normalize();
	row4.normalize();
	setData(row1,row2,row3,row4);
}

void NSMatrix4Df::setData(const NSVec4Df & row1,const NSVec4Df & row2,const NSVec4Df & row3, const NSVec4Df & row4)
{
	data[0][0] = row1.x; data[0][1] = row1.y; data[0][2] = row1.z; data[0][3] = row1.w;
	data[1][0] = row2.x; data[1][1] = row2.y; data[1][2] = row2.z; data[1][3] = row2.w;
	data[2][0] = row3.x; data[2][1] = row3.y; data[2][2] = row3.z; data[2][3] = row3.w;
	data[3][0] = row4.x; data[3][1] = row4.y; data[3][2] = row4.z; data[3][3] = row4.w;
}

void NSMatrix4Df::setData(const NSVec3Df & row1,const NSVec3Df & row2,const NSVec3Df & row3)
{
	setRow(NSVec4Df(row1.x,row1.y,row1.z,0.0f),0);
	setRow(NSVec4Df(row2.x,row2.y,row2.z,0.0f),1);
	setRow(NSVec4Df(row3.x,row3.y,row3.z,0.0f),2);
}

void NSMatrix4Df::setRow(const NSVec4Df & vec, int row)
{
	if (row < 0 || row >= rows)
	{
		LogFile("Math3D error : NSMatrix4Df::setRow row index out of range");
		return;
	}
	data[row][0] = vec.x;
	data[row][1] = vec.y;
	data[row][2] = vec.z;
	data[row][3] = vec.w;
}

void NSMatrix4Df::setColumn(const NSVec4Df & vec, int column)
{
	if (column < 0 || column >= columns)
	{
		LogFile("Math3D error : NSMatrix4Df::setColumn column index out of range");
		return;
	}
	data[0][column] = vec.x;
	data[1][column] = vec.y;
	data[2][column] = vec.z;
	data[3][column] = vec.w;
}

float & NSMatrix4Df::value(int row, int column)
{
	return data[row][column];
}

const float & NSMatrix4Df::value(int row, int column) const
{
	return data[row][column];
}

NSVec4Df NSMatrix4Df::getRow(int row) const
{
	if (row < 0 || row >= rows)
	{
		LogFile("Math3D error : NSMatrix4Df::getRow row index out of range");
		return NSVec4Df();
	}
	return NSVec4Df(data[row][0], data[row][1], data[row][2], data[row][3]);
}

NSVec4Df NSMatrix4Df::getColumn(int column) const
{
	if (column < 0 || column >= columns)
	{
		LogFile("Math3D error : NSMatrix4Df::getColumn column index out of range");
		return NSVec4Df();
	}
	return NSVec4Df(data[0][column], data[1][column], data[2][column], data[3][column]);
}

void NSMatrix4Df::rotate(const NSVec3Df & amount)
{
	(*this) *= getRotationMatrix(amount);
}

void NSMatrix4Df::rotate(float xAxisAng, float yAxisAng, float zAxisAng)
{
	(*this) *= getRotationMatrix(xAxisAng,yAxisAng,zAxisAng);
}

void NSMatrix4Df::translate(const NSVec3Df & amount)
{
	(*this) *= getTranslationMatrix(amount);
}

void NSMatrix4Df::translate(float xMove, float yMove, float zMove)
{
	(*this) *= getTranslationMatrix(xMove,yMove,zMove);
}

void NSMatrix4Df::scale(const NSVec3Df & amount)
{
	(*this) *= getScalingMatrix(amount);
}

void NSMatrix4Df::scale(float scaleX, float scaleY, float scaleZ)
{
	(*this) *= getScalingMatrix(scaleX,scaleY,scaleZ);
}

float * NSMatrix4Df::getDataPtr()
{
	return &data[0][0];
}

float NSMatrix4Df::getDeterminate() const
{
	return value(0,0)*value(1,1)*value(2,2)*value(3,3) + value(0,0)*value(1,2)*value(2,3)*value(3,1) + value(0,0)*value(1,3)*value(2,1)*value(3,2)
		 + value(0,1)*value(1,0)*value(2,3)*value(3,2) + value(0,1)*value(1,2)*value(2,0)*value(3,3) + value(0,1)*value(1,3)*value(2,2)*value(3,0)
		 + value(0,2)*value(1,0)*value(2,1)*value(3,3) + value(0,2)*value(1,1)*value(2,3)*value(3,0) + value(0,2)*value(1,3)*value(2,0)*value(3,1)
		 + value(0,3)*value(1,0)*value(2,2)*value(3,1) + value(0,3)*value(1,1)*value(2,0)*value(3,2) + value(0,3)*value(1,2)*value(2,1)*value(3,0)
		 - value(0,0)*value(1,1)*value(2,3)*value(3,2) - value(0,0)*value(1,2)*value(2,1)*value(3,3) - value(0,0)*value(1,3)*value(2,2)*value(3,1)
		 - value(0,1)*value(1,0)*value(2,2)*value(3,3) - value(0,1)*value(1,2)*value(2,3)*value(3,0) - value(0,1)*value(1,3)*value(2,0)*value(3,2)
		 - value(0,2)*value(1,0)*value(2,3)*value(3,1) - value(0,2)*value(1,1)*value(2,0)*value(3,3) - value(0,2)*value(1,3)*value(2,1)*value(3,0)
		 - value(0,3)*value(1,0)*value(2,1)*value(3,2) - value(0,3)*value(1,1)*value(2,2)*value(3,0) - value(0,3)*value(1,2)*value(2,2)*value(3,1);
}

NSMatrix4Df NSMatrix4Df::getInverse() const
{
	float det = getDeterminate();
	NSMatrix4Df ret;
	if (det != 0)
	{
		ret.value(0,0) = (value(1,1)*value(2,2)*value(3,3) + value(1,2)*value(2,3)*value(3,1) + value(0,3)*value(2,1)*value(3,2)
					    - value(1,1)*value(2,3)*value(3,2) - value(1,2)*value(2,1)*value(3,3) - value(0,3)*value(2,2)*value(3,1)) / det;
		ret.value(0,1) = (value(0,1)*value(2,3)*value(3,2) + value(0,2)*value(2,1)*value(3,3) + value(0,3)*value(2,2)*value(3,1)
					    - value(0,1)*value(2,2)*value(3,3) - value(0,2)*value(2,3)*value(3,1) - value(0,3)*value(2,1)*value(3,2)) / det;
		ret.value(0,2) = (value(0,1)*value(1,2)*value(3,3) + value(0,2)*value(1,3)*value(3,1) + value(0,3)*value(1,1)*value(3,2)
					    - value(0,1)*value(1,3)*value(3,2) - value(0,2)*value(1,1)*value(3,3) - value(0,3)*value(1,2)*value(3,1)) / det;
		ret.value(0,3) = (value(0,1)*value(1,3)*value(2,2) + value(0,2)*value(1,1)*value(2,3) + value(0,3)*value(1,2)*value(2,1)
					    - value(0,1)*value(1,2)*value(2,3) - value(0,2)*value(1,3)*value(2,1) - value(0,3)*value(1,1)*value(2,2)) / det;
		ret.value(1,0) = (value(1,0)*value(2,3)*value(3,2) + value(1,2)*value(2,0)*value(3,3) + value(1,3)*value(2,2)*value(3,0)
					    - value(1,0)*value(2,2)*value(3,3) - value(1,2)*value(2,3)*value(3,0) - value(1,3)*value(2,0)*value(3,2)) / det;
		ret.value(1,1) = (value(0,0)*value(2,2)*value(3,3) + value(0,2)*value(2,3)*value(3,0) + value(0,3)*value(2,0)*value(3,2)
					    - value(0,0)*value(2,3)*value(3,2) - value(0,2)*value(2,0)*value(3,3) - value(0,3)*value(2,2)*value(3,0)) / det;
		ret.value(1,2) = (value(0,0)*value(1,3)*value(3,2) + value(0,2)*value(1,0)*value(3,3) + value(0,3)*value(1,2)*value(3,0)
					    - value(0,0)*value(1,2)*value(3,3) - value(0,2)*value(1,3)*value(3,0) - value(0,3)*value(1,0)*value(3,2)) / det;
		ret.value(1,3) = (value(0,0)*value(1,2)*value(2,3) + value(0,2)*value(1,3)*value(2,0) + value(0,3)*value(1,0)*value(2,2)
					    - value(0,0)*value(1,3)*value(2,2) - value(0,2)*value(1,0)*value(2,3) - value(0,3)*value(1,2)*value(2,0)) / det;
		ret.value(2,0) = (value(1,0)*value(2,1)*value(3,3) + value(1,1)*value(2,3)*value(3,0) + value(1,3)*value(2,0)*value(3,1)
					    - value(1,0)*value(2,3)*value(3,1) - value(1,1)*value(2,0)*value(3,3) - value(1,3)*value(2,1)*value(3,0)) / det;
		ret.value(2,1) = (value(0,0)*value(2,3)*value(3,1) + value(0,1)*value(2,0)*value(3,3) + value(0,3)*value(2,1)*value(3,0)
					    - value(0,0)*value(2,1)*value(3,3) - value(0,1)*value(2,3)*value(3,0) - value(0,3)*value(2,0)*value(3,1)) / det;
		ret.value(2,2) = (value(0,0)*value(1,1)*value(3,3) + value(0,1)*value(1,3)*value(3,0) + value(0,3)*value(1,0)*value(3,1)
					    - value(0,0)*value(1,3)*value(3,1) - value(0,1)*value(1,0)*value(3,3) - value(0,3)*value(1,1)*value(3,0)) / det;
		ret.value(2,3) = (value(0,0)*value(1,3)*value(2,1) + value(0,1)*value(1,0)*value(2,3) + value(0,3)*value(1,1)*value(2,0)
					    - value(0,0)*value(1,1)*value(2,3) - value(0,1)*value(1,3)*value(2,0) - value(0,3)*value(1,0)*value(2,1)) / det;
		ret.value(3,0) = (value(1,0)*value(2,2)*value(3,1) + value(1,1)*value(2,0)*value(3,2) + value(1,2)*value(2,1)*value(3,0)
					    - value(1,0)*value(2,1)*value(3,2) - value(1,1)*value(2,2)*value(3,0) - value(1,2)*value(2,0)*value(3,1)) / det;
		ret.value(3,1) = (value(0,0)*value(2,1)*value(3,2) + value(0,1)*value(2,2)*value(3,0) + value(0,2)*value(2,0)*value(3,1)
					    - value(0,0)*value(2,2)*value(3,1) - value(0,1)*value(2,0)*value(3,2) - value(0,2)*value(2,1)*value(3,0)) / det;
		ret.value(3,2) = (value(0,0)*value(1,2)*value(3,1) + value(0,1)*value(1,0)*value(3,2) + value(0,2)*value(1,1)*value(3,0)
					    - value(0,0)*value(1,1)*value(3,2) - value(0,1)*value(1,2)*value(3,0) - value(0,2)*value(1,0)*value(3,1)) / det;
		ret.value(3,3) = (value(0,0)*value(1,1)*value(2,2) + value(0,1)*value(1,2)*value(2,0) + value(0,2)*value(1,0)*value(2,1)
					    - value(0,0)*value(1,2)*value(2,1) - value(0,1)*value(1,0)*value(2,2) - value(0,2)*value(1,1)*value(2,0)) / det;
	}
	else
		LogFile("No inverse found for matrix..");
	return ret;
}

std::string NSMatrix4Df::toString() const
{
	std::stringstream ss;
	ss << "\nx : " << value(0,0) << "  y : " << value(0,1) << "  z : " << value(0,2) << "  w : " << value(0,3) << "\n";
	ss << "x : " << value(1,0) << "  y : " << value(1,1) << "  z : " << value(1,2) << "  w : " << value(1,3) << "\n";
	ss << "x : " << value(2,0) << "  y : " << value(2,1) << "  z : " << value(2,2) << "  w : " << value(2,3) << "\n";
	ss << "x : " << value(3,0) << "  y : " << value(3,1) << "  z : " << value(3,2) << "  w : " << value(3,3) << "\n";
	return ss.str();
}

// static functions
NSMatrix4Df NSMatrix4Df::getRotationMatrix(const NSVec3Df & rotation)
{
	return getRotationMatrix(rotation.x,rotation.y,rotation.z);
}

NSMatrix4Df NSMatrix4Df::getRotationMatrix(float xAxisAng, float yAxisAng, float zAxisAng)
{
	float xRad = DegreesToRadians(xAxisAng);
	float yRad = DegreesToRadians(yAxisAng);
	float zRad = DegreesToRadians(zAxisAng);
	
	NSMatrix4Df xRotMat;
	NSMatrix4Df yRotMat;
	NSMatrix4Df zRotMat;

	xRotMat.value(1,1) = cosf(xRad); xRotMat.value(1,2) = -sinf(xRad);
	xRotMat.value(2,1) = sinf(xRad); xRotMat.value(2,2) = cosf(xRad);

	yRotMat.value(0,0) = cosf(yRad); yRotMat.value(0,2) = -sinf(yRad);
	yRotMat.value(2,0) = sinf(yRad); yRotMat.value(2,2) = cosf(yRad);

	zRotMat.value(0,0) = cosf(zRad); zRotMat.value(0,1) = -sinf(zRad);
	zRotMat.value(1,0) = sinf(zRad); zRotMat.value(1,1) = cosf(zRad);

	return zRotMat * yRotMat * xRotMat;
}

NSMatrix4Df NSMatrix4Df::getTranslationMatrix(const NSVec3Df & translation)
{
	return getTranslationMatrix(translation.x,translation.y,translation.z);
}

NSMatrix4Df NSMatrix4Df::getTranslationMatrix(float xMove, float yMove, float zMove)
{
	NSMatrix4Df identity;
	// set the fourth column of the matrix to the translation amount
	// the rest of the matrix is the identity matrix
	identity.setColumn(NSVec4Df(xMove,yMove,zMove,1),3);
	return identity;
}

NSMatrix4Df NSMatrix4Df::getScalingMatrix(const NSVec3Df & scaling)
{
	return getScalingMatrix(scaling.x,scaling.y,scaling.z);
}

NSMatrix4Df NSMatrix4Df::getScalingMatrix(float scaleX, float scaleY, float scaleZ)
{
	NSMatrix4Df identity;
	// replace 1s in identity with scaling factors here
	identity.value(0,0) = scaleX;
	identity.value(1,1) = scaleY;
	identity.value(2,2) = scaleZ;
	return identity;
}

NSMatrix4Df NSMatrix4Df::getProjectionMatrix(float fovAngle, int screenWidth, int screenHeight, float zNear, float zFar)
{
	float aspectRatio = float(screenWidth) / float(screenHeight);
	float zRange = zNear - zFar;
	float usefulNum = tanf(DegreesToRadians(fovAngle) / 2.0f);
	NSMatrix4Df projMat;

	projMat.value(0,0) = ( 1.0f / (aspectRatio * usefulNum) );
	projMat.value(1,1) = ( 1.0f / usefulNum );
	projMat.value(2,2) = (-zNear - zFar) / zRange;
	projMat.value(2,3) = (2.0f * zFar * zNear) / zRange;
	projMat.value(3,2) = 1.0f;
	projMat.value(3,3) = 0.0f;

	return projMat;
}


// Overloaded operators
NSMatrix4Df NSMatrix4Df::operator*(const NSMatrix4Df & rhsMatrix) const
{
	NSMatrix4Df matrix;
	for (int count = 0; count < rows; ++count)
	{
		for (int i = 0; i < rows; ++i)
		{
			float val = 0.0f;
			for (int j = 0; j < columns; ++j)
				 val += data[count][j] * rhsMatrix.data[j][i];

			matrix.data[count][i] = val;
		}
	}
	return matrix;
}

NSMatrix4Df NSMatrix4Df::operator+(const NSMatrix4Df & rhsMatrix) const
{
	NSMatrix4Df matrix;
	for (int i = 0; i < rows; ++i)
	{
		for (int j = 0; j < columns; ++j)
			matrix.data[i][j] = data[i][j] + rhsMatrix.data[i][j];
	}
	return matrix;
}

NSMatrix4Df NSMatrix4Df::operator-(const NSMatrix4Df & rhsMatrix) const
{
	NSMatrix4Df matrix;
	for (int i = 0; i < rows; ++i)
	{
		for (int j = 0; j < columns; ++j)
			matrix.data[i][j] = data[i][j] - rhsMatrix.data[i][j];
	}
	return matrix;
}

bool NSMatrix4Df::operator==(const NSMatrix4Df & rhsMatrix) const
{
	for (int i = 0; i < rows; ++i)
	{
		for (int j = 0; j < columns; ++j)
		{
			if (data[i][j] != rhsMatrix.data[i][j])
				return false;
		}
	}
	return true;
}

bool NSMatrix4Df::operator!=(const NSMatrix4Df & rhsMatrix) const
{
	return !( (*this) == rhsMatrix );
}

NSMatrix4Df & NSMatrix4Df::operator=(const NSMatrix4Df & rhsMatrix)
{
	if (this == &rhsMatrix)
		return *this;

	for (int i = 0; i < rows; ++i)
	{
		for (int j = 0; j < columns; ++j)
			value(i,j) = rhsMatrix.value(i,j);
	}
	return *this;
}

NSMatrix4Df & NSMatrix4Df::operator+=(const NSMatrix4Df & rhsMatrix)
{
	return ( (*this) = ( (*this) + rhsMatrix ) );
}

NSMatrix4Df & NSMatrix4Df::operator-=(const NSMatrix4Df & rhsMatrix)
{
	return ( (*this) = ( (*this) - rhsMatrix ) );
}

NSMatrix4Df & NSMatrix4Df::operator*=(const NSMatrix4Df & rhsMatrix)
{
	return ( (*this) = ( (*this) * rhsMatrix ) );
}

// Overloaded for 4D vector
NSVec4Df NSMatrix4Df::operator*(const NSVec4Df & rhsVec) const
{
	NSVec4Df vec;

	float orig[MATDIM4D];
	float arr[MATDIM4D];
	orig[0] = rhsVec.x; orig[1] = rhsVec.y; orig[2] = rhsVec.z; orig[3] = rhsVec.w;
	for (int i = 0; i < rows; ++i)
	{
		float val = 0.0f;
		for (int j = 0; j < columns; ++j )
			val += data[i][j] * orig[j];

		arr[i] = val;
	}

	return NSVec4Df(arr[0], arr[1], arr[2],arr[3]);
}

NSVec3Df NSMatrix4Df::operator*(const NSVec3Df & rhsVec) const
{
	NSVec4Df vec(rhsVec,1.0);

	vec = (*this) * vec;

	return NSVec3Df(vec.x,vec.y,vec.z);
}

float RadiansToDegrees(float radians)
{
	float PI = 3.14159265359;
	return radians * (180.0f / PI);
}

float DegreesToRadians(float degrees)
{
	float PI = 3.14159265359;
	return degrees * ( PI / 180.0f);
}